#include "assist_land.h"
#include <cmath>
#include <chrono>
#include "ekf.h"
#include <limits>



using namespace std;
using namespace cv;


#define SBUF_SIZE 512

// the longth of the external marker long edge is approximately 0.8 meters
#define EXTERNAL_LONG_EDGE (0.85f)
// the longth of the internal marker long edge is approximately 0.13 meters
#define INTERNAL_LONG_EDGE (0.10f)


#define LIVE_STREAM_FRAME_RATE 25

#define START_CONTROL_FLIGHT_HEIGHT (80.0f)

#define CAMERA_SCAN_GIMBAL_ANGLE	(-60.0f)  //range: from -90 to -45

#define GIMBAL_ROTATE_SPEED_ANGLE	(10.0f)

#define START_CAMERA_SCAN_MODE		(8.0f)

#define IS_LAND_QR_OK_NUM		  5

#define CAMARE_TO_FLIGHT_CENTER_DISTANCE  (0.10f)

#define FLIGHT_BEARING_LOCK_NUM   5

#define EKF_INIT_POSERROR_NUM	  5

#define FINISH_XY_CONTROL_DISTANCE (0.05f)

#define SET_Z_VELOCITY_ZERO_XY_DISTANCE (1.0f)

#define LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO	12   // 12%

#define LOW_PASS_FILTER_ALPHA (0.3f)   //rang 0.0f- 1.0f





enum measureDimUpdateState{
	measureDimUpdateX = 0,
	measureDimUpdateY = 1
};


template <class T>
void lowPassFilter(T &output, T sample, float alpha)
{
	output += (sample - output) * alpha;
}


AssistLand::AssistLand():
	m_apmFlightHandler(NULL),
	m_assistLandModeRunFlag(false),
	m_goToAlternateLandPoint(false),
	m_ekfInitSuccess(false),
	m_startCameraScanMode(false),
	m_siftHanderStatus(siftMarkerNone),
	m_needAlignCameraYaw(false),
	m_positionErrorUpdateFlag(false),
	m_homeBearingLockFlag(false),
	m_setLandPosStatus(notInit)
{
	resetFlag();
}


AssistLand::~AssistLand()
{


}

void AssistLand::init(apmFlightHandler *handler)
{
	m_apmFlightHandler = handler;
}

void AssistLand::writeLogFile(const char * format, ...)
{
	char sbuf[SBUF_SIZE];
	va_list args;
	va_start (args, format);
	vsnprintf (sbuf,SBUF_SIZE,format, args);
	va_end (args);

	m_apmFlightHandler->writeLogFile("%s", sbuf);
}



void AssistLand::assistLandRun()
{
	if(NULL == m_apmFlightHandler)
	{
		return;
	}

	m_assistLandModeRunFlag = true;

	while(m_assistLandModeRunFlag)
	{
		int64_t updateTimeStamp;

		m_apmFlightHandler->getFlightMode(m_flightCurrentMode, updateTimeStamp);
		m_apmFlightHandler->getFlightArmStatus(m_armStatus, updateTimeStamp);
		m_apmFlightHandler->getFlightSystemStatus(m_flightSystemStatus, updateTimeStamp);

		switch(getFlightStatus())
		{
			case MAV_LANDED_STATE_ON_GROUND:
				resetFlag();
				break;

			case MAV_LANDED_STATE_IN_AIR:
				break;

			case MAV_LANDED_STATE_TAKEOFF:
				break;

			case MAV_LANDED_STATE_LANDING:
				break;

			default:
				break;
		}

		switch(m_flightCurrentMode)
		{
			case STABILIZE: // 0
				resetFlag();
				break;

			case ALT_HOLD:// 2
				resetFlag();
				break;

			case AUTO: // 3
				resetFlag();
				break;

			// assist land mode, control by 3588 board
			case GUIDED: // 4
				guidedModeHandler();
				break;

			case LOITER: // 5
				resetFlag();
				break;

			case RTL: // 6
			{
				if(getFlightStatus() == MAV_LANDED_STATE_LANDING)
				{
					assistLand();
				}
				else
				{
					resetFlag();
				}
				break;
			}

			case LAND: // 9
				assistLand();
				break;

			default:
				resetFlag();
				break;
		}

		sleepMs(1000);
		printf("-------assist land run--------\n");
	}
}

void *assistLandThreadRun(void *arg)
{
	assistLand.controlFlightLand();
	return NULL;
}

void *siftThreadRun(void *arg)
{
	assistLand.computePositionError();
	return NULL;
}

void AssistLand::assistLand()
{
	if(m_flightSystemStatus != MAV_STATE_ACTIVE)
	{
		writeLogFile("assistLand flightSystemStatus is error\n");
		return;
	}

	if(getFlightCurrentHeigh() > START_CONTROL_FLIGHT_HEIGHT)
	{
		writeLogFile("height-error:%f\n", getFlightCurrentHeigh());
		return;
	}

	if(m_goToAlternateLandPoint)
	{
		return;
	}

	if(m_assistLandRunFunctionFlag && m_siftRunFunctionFlag)
	{
		return;
	}
	else
	{
		m_assistLandRunFunctionFlag = false;
		m_siftRunFunctionFlag = false;
	}

	if(!startControlFlight())
	{
		goToAlternateLandingPoint();
		return;
	}

	int ret = pthread_create(&m_landRunTid, NULL, assistLandThreadRun, NULL);
	if(0 != ret)
	{
		writeLogFile("assistLand assistLandRun pthread_create err\n");
		return;
	}

	ret = pthread_create(&m_siftRunTid, NULL, siftThreadRun, NULL);
	if(0 != ret)
	{
		m_assistLandRunFunctionFlag = false;
		writeLogFile("assistLand siftRun pthread_create err\n");
		return;
	}

}

double AssistLand::getFlightCurrentHeigh()
{
	int64_t updateTimeStamp;
	float flightRelativeAlt;
	m_apmFlightHandler->getRelativeAlt(flightRelativeAlt, updateTimeStamp);
	return flightRelativeAlt;
}


void AssistLand::resetFlag()
{
	m_assistLandRunFunctionFlag = false;
	m_siftRunFunctionFlag = false;
}

bool AssistLand::startControlFlight()
{
	if(0 != access("/home/forlinx/hbSource/external_marker.png", F_OK) 				||
		0 != access("/home/forlinx/hbSource/internal_marker.png", F_OK) 			||
		0 != access("/home/forlinx/hbSource/alternate_external_marker.png", F_OK) 	||
		0 != access("/home/forlinx/hbSource/alternate_internal_marker.png", F_OK))
	{
		writeLogFile("startControlFlight /home/forlinx/hbSource/external_marker.png may be not exist\n");
		return false;
	}

	if(m_apmFlightHandler->getLiveViewHealthy() == false)
	{
		writeLogFile("startControlFlight  getLiveViewHealthy is false\n");
		return false;
	}

	if(!m_apmFlightHandler->setFlightMode(GUIDED))
	{
		writeLogFile("startControlFlight setFlightMode GUIDED failed\n");
		return false;
	}

	return true;
}


bool AssistLand::goToAlternateLandingPoint()
{

	// switch to guided mode, use positon guid
	if(m_flightCurrentMode == GUIDED)
	{
		m_goToAlternateLandPoint = true;
		return true;
	}
	else if(m_apmFlightHandler->setFlightMode(GUIDED))
	{
		m_goToAlternateLandPoint = true;
		return true;
	}
	else
	{
		return false;
	}
}


void ekfLinearFilter(double *u, double* x, double dt,double *Xdot, double* F)
{
	/* state prediction
     positionErrorSum -= (v_start + v_end) * dt / 2
     velocity  += v_start + a*dt
  	*/

	//u[0] is vx end, u[1] is vy end

  	//positionErrorSum .x xdot -->vx
	Xdot[0] = -(u[0] + x[2]) / 2.0f;
	//positionErrorSum .y xdot -->vy
	Xdot[1] = -(u[1] + x[3]) / 2.0f;

	//velocity .x xdot--->ax
	Xdot[2] = (u[0] - x[2]) / dt;

	//velocity .y xdot--->ay
	Xdot[3] = (u[1] - x[3]) / dt;

	/* F is the Jacobian of xdot wrt the state
		F[0*4+0] = d(Xdot[0]) / d(x[0]);
		F[0*4+1] = d(Xdot[0]) / d(x[1]);
		F[0*4+2] = d(Xdot[0]) / d(x[2]);
		F[0*4+3] = d(Xdot[0]) / d(x[3]);

		F[1*4+0] = d(Xdot[1]) / d(x[0]);
		F[1*4+1] = d(Xdot[1]) / d(x[1]);
		F[1*4+2] = d(Xdot[1]) / d(x[2]);
		F[1*4+3] = d(Xdot[1]) / d(x[3]);

		F[2*4+0] = d(Xdot[2]) / d(x[0]);
		F[2*4+1] = d(Xdot[2]) / d(x[1]);
		F[2*4+2] = d(Xdot[2]) / d(x[2]);
		F[2*4+3] = d(Xdot[2]) / d(x[3]);

		F[3*4+0] = d(Xdot[3]) / d(x[0]);
		F[3*4+1] = d(Xdot[3]) / d(x[1]);
		F[3*4+2] = d(Xdot[3]) / d(x[2]);
		F[3*4+3] = d(Xdot[3]) / d(x[3]);
	*/
	F[0*4+0] = 0.;
	F[0*4+1] = 0.;
	F[0*4+2] = -0.5f;
	F[0*4+3] = 0.;

	F[1*4+0] = 0.;
	F[1*4+1] = 0.;
	F[1*4+2] = 0.;
	F[1*4+3] = -0.5f;

	F[2*4+0] = 0.;
	F[2*4+1] = 0.;
	F[2*4+2] = -1/dt;
	F[2*4+3] = 0.;

	F[3*4+0] = 0.;
	F[3*4+1] = 0.;
	F[3*4+2] = 0.;
	F[3*4+3] = -1/dt;
}


void ekfLinearMeasure(double *y, double* err, double*x, double *H, int measureDimUpdate)
{
	/* err = measure - estimate
	 	H is the jacobian of the measure wrt X
	 	H[0] = d(y[0])/d(x[0]);
	 	H[1] = d(y[0])/d(x[1]);
	 	H[2] = d(y[0])/d(x[2]);
	 	H[3] = d(y[0])/d(x[3]);
	*/
	switch(measureDimUpdate)
	{
		case measureDimUpdateX:
		{
			*err = y[0] - x[0];
			H[0] = 1.;
			H[1] = 0.;
			H[2] = 0.;
			H[3] = 0.;
			break;
		}
		case measureDimUpdateY:
		{
			*err = y[0] - x[1];
			H[0] = 0.;
			H[1] = 1.;
			H[2] = 0.;
			H[3] = 0.;
			break;
		}
	}
}


void AssistLand::controlFlightLand()
{
	Vector3f translationXy;
	Vector3f latestVelocity;
	int64_t latestVelocityTimestamp;
	Vector3f currentVelocity;
	int64_t currentVelocityTimestamp;
	bool velocityUpdateFlag;

	double dt;
	std::chrono::steady_clock::time_point latestTime;
	std::chrono::steady_clock::time_point currentTime;

	Vector3f positionErrorSum;
	bool positionErrorSumInitFlag;
	positionErrorSumInitFlag = false;

	bool runControlFlightFlag;
	runControlFlightFlag = false;
	memset(&positionErrorSum, 0, sizeof(Vector3f));

	m_assistLandRunFunctionFlag = true;

	//pid
	PID yawPid(1.0f, 0.1f, 0.01f, 500);
	PID txPid(2.5f, 0.25f, 0.02f, 2);
	PID tyPid(2.5f, 0.25f, 0.02f, 2);

	//ekf
	// initial X status: pos x, pos y, vel x, vel y
  	double X[4] = {0.0, 0.0, 0.0, 0.0};
	// initial state covariance matrix
  	double P[4*4] = {1.0, 0.0, 0.0, 0.0,
  					  0.0, 1.0, 0.0, 0.0,
  					  0.0, 0.0, 1.0, 0.0,
  					  0.0, 0.0, 0.0, 1.0};
	// model noise covariance matrix
	double Q[4*4]={1.5, 0.0, 0.0, 0.0,
				   0.0, 1.5, 0.0, 0.0,
				   0.0, 0.0, 1.5, 0.0,
				   0.0, 0.0, 0.0, 1.5};
	// measurement noise covariance matrix
	double R[1]={0.02};
	// measure: this is sift compute posError x y
  	double y[1];
	//command: this flight velocity x y
	double u[2];

	struct ekf_filter* filter;
  	filter = ekf_filter_new(4, 1, Q, R, ekfLinearFilter, ekfLinearMeasure);

	getFlightVelocity(latestVelocity, latestVelocityTimestamp);
	latestTime = std::chrono::steady_clock::now();
	velocityUpdateFlag = false;

	vector<Vector3f> ekfInitPosError;

	Vector3f velocityOutput;
	double yawRateOutput = 0.0f;
	memset(&velocityOutput, 0, sizeof(Vector3f));

	while(m_assistLandRunFunctionFlag)
	{
		//wait for System scheduling
		sleepMs(2);

		//compute the xy translationSum
		getFlightVelocity(currentVelocity, currentVelocityTimestamp);

		if(currentVelocityTimestamp > latestVelocityTimestamp)
		{
			currentTime = std::chrono::steady_clock::now();

			//get the dt, unit: s
			dt = std::chrono::duration_cast<std::chrono::duration<double>>(currentTime - latestTime).count();

			//compute the north and east translation: s = (v_start + v_end) * dt / 2
			translationXy.x = (currentVelocity.x  + latestVelocity.x) * dt / 2;
			translationXy.y = (currentVelocity.y  + latestVelocity.y) * dt / 2;
			translationXy.z = 0;

			//update latestTime and latestVelocity
			latestTime = currentTime;
			memcpy(&latestVelocity, &currentVelocity, sizeof(Vector3f));
			memcpy(&latestVelocityTimestamp, &currentVelocityTimestamp, sizeof(int64_t));

			//update the translation sum
			setTranslationSum(translationXy);

			velocityUpdateFlag = true;
		}

		Vector3f positionErr;
		bool updateFlag;
		//get the position distance of sift computing.
		getPositionError(positionErr, updateFlag);

		if(updateFlag)
		{
			if(!positionErrorSumInitFlag)
			{
				ekfInitPosError.push_back(positionErr);
				if(ekfInitPosError.size() > EKF_INIT_POSERROR_NUM ||
					(m_startCameraScanMode && ekfInitPosError.size() > 0))
				{
					memset(&positionErr, 0, sizeof(Vector3f));
					for(int i=0; i<ekfInitPosError.size(); i++)
					{
						positionErr.x += ekfInitPosError[i].x;
						positionErr.y += ekfInitPosError[i].y;
					}

					positionErr.x /= (float)ekfInitPosError.size();
					positionErr.y /= (float)ekfInitPosError.size();

					//ekf init
					X[0] = positionErr.x;
					X[1] = positionErr.y;
					X[2] = currentVelocity.x;
					X[3] = currentVelocity.y;

					ekf_filter_reset(filter, X, P);

					positionErrorSum.x = X[0];
					positionErrorSum.y = X[1];

					writeLogFile("assistLandRun-ekf-init x[0]:%f, x[1]:%f, x[2]:%f, x[3]:%f\n", X[0], X[1], X[2], X[3]);

					m_ekfInitSuccess = true;

					positionErrorSumInitFlag = true;
					velocityUpdateFlag = false;
				}
			}
			else
			{
				//update x measure
				y[0] = positionErr.x;
				ekf_filter_update(filter, y, measureDimUpdateX);

				//update y measure
				y[0] = positionErr.y;
				ekf_filter_update(filter, y, measureDimUpdateY);

				//get ekf state
				ekf_filter_get_state(filter, X, P);

				positionErrorSum.x = X[0];
				positionErrorSum.y = X[1];
				//writeLogFile("assistLandRun-ekf x[0]:%f, x[1]:%f, x[2]:%f, x[3]:%f, positionErr1 x:%f, y:%f, currentVelocity.x:%f, y%f\n",
							//X[0], X[1], X[2], X[3], positionErr.x, positionErr.y, currentVelocity.data.x, currentVelocity.data.y);
			}
		}

		Vector3f velocity;
		double yawRate;
		double angleError;
		angleError = 0.0f;

		//control yaw
		yawRate = controlYaw(yawPid, angleError);

		//control z
		velocity.z = controlVelocityZ();

		double xyDistance;
		xyDistance = 0.0f;
		if(positionErrorSumInitFlag && !m_needAlignCameraYaw)
		{
			if(velocityUpdateFlag && dt > 0)
			{
				velocityUpdateFlag = false;
				//update ekf predict
				u[0] = currentVelocity.x;
				u[1] = currentVelocity.y;

				ekf_filter_predict(filter, u, dt);
				ekf_filter_get_state(filter, X, P);

				positionErrorSum.x = X[0];
				positionErrorSum.y = X[1];
			}

			//control xy
			controlVelocityXY(txPid, tyPid, positionErrorSum, velocity);

			//control z according to the distance of xy
			xyDistance = sqrt(pow(positionErrorSum.x, 2) + pow(positionErrorSum.y, 2));
			if(xyDistance > SET_Z_VELOCITY_ZERO_XY_DISTANCE)
			{
				if(getBatteryInfoPercentage() > LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO)
				{
					velocity.z = 0.0f;
				}
			}
		}
		else
		{
			velocity.x = 0.0f;
			velocity.y = 0.0f;
			txPid.resetI();
			tyPid.resetI();
		}

		//low pass filter can make the plane run smooth
		lowPassFilter<float>(velocityOutput.x, velocity.x, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<float>(velocityOutput.y, velocity.y, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<float>(velocityOutput.z, velocity.z, LOW_PASS_FILTER_ALPHA);
		lowPassFilter<double>(yawRateOutput, yawRate, LOW_PASS_FILTER_ALPHA);

		velocityControlToCopter(velocityOutput, yawRateOutput);

		//assistLand.writeLogFile("assistLandRun  distanceError x:%f, y:%f, controlvelocity:x:%f, y:%f, z:%f, yawRate:%f\n",
													//positionErrorSum.x, positionErrorSum.y, velocity.x, velocity.y, velocity.z, yawRate);
	}
}

void AssistLand::computePositionError()
{
	bool ret;

	m_siftRunFunctionFlag = true;

	//first, gimbal reset to align with the flight head
	//second, rotate the pitch from 0 degree to -90 dgree.
	gimbalSetModeAndReset();

	double differenceGimbalYawAndFlightYaw;
	//recode the diffrence of between gimbal yaw with flight yaw, this data can be use for setting the flight landing bearing.
	differenceGimbalYawAndFlightYaw = getFlightCurrentBearing() - getGimbalAnglesYaw();

	m_siftHanderStatus = siftMarkerNone;
	int siftCount = 0;

	vector<KeyPoint> markerExKp, markerInKp;
	Mat markerExDes, markerInDes;

	//handler the external marker and internal marker by sift
	siftHandlerMarker(markerExKp, markerExDes, markerInKp, markerInDes, siftLandMarker);

	Mat im;
	Mat imGray;
	vector<float> guessBearing;
	double intervalMs = (1.0f / (float)LIVE_STREAM_FRAME_RATE) * 1e3;
	int delay = 2;

	//camera scan mode
	bool scanOver = false;
	double lastGimbalYaw;
	double currentGimbalAngle;
	int64_t imageLastUpdateTimeStamp;

	m_startCameraScanMode = false;
	m_ekfInitSuccess = false;
	m_needAlignCameraYaw = false;

	imageLastUpdateTimeStamp = 0;
	while(m_siftRunFunctionFlag)
	{
		//wait for System scheduling
		sleepMs(delay);
		resetTranslationSum();

		//get new liveview
		int64_t updateTimeStamp;
		float imageFlightYaw;
		Vector3f imageGimbalAngle;
		m_apmFlightHandler->getLiveViewImage(im, imageFlightYaw, imageGimbalAngle, updateTimeStamp);

		if(imageLastUpdateTimeStamp == 0 || imageLastUpdateTimeStamp >= updateTimeStamp)
		{
			delay = 2;
			imageLastUpdateTimeStamp = updateTimeStamp;
			continue;
		}

		//update image last time stamp
		imageLastUpdateTimeStamp = updateTimeStamp;

		if((imageGimbalAngle.x < -91.0f || imageGimbalAngle.x > -89.0f) && (!m_startCameraScanMode))
		{
			//confirm the gimbal camera head is down
			gimbalRotate();
			writeLogFile("siftRun gimbalManagerRotate-90 x:%f, y:%f, z:%f\n", imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);
			delay = intervalMs;
			continue;
		}

		if(m_startCameraScanMode)
		{
			//camera scan angle CAMERA_SCAN_GIMBAL_ANGLE
			if((imageGimbalAngle.x < CAMERA_SCAN_GIMBAL_ANGLE - 1 || imageGimbalAngle.x > CAMERA_SCAN_GIMBAL_ANGLE + 1))
			{
				cameraScanModeRotateAbsoluteAngle(0.0f);
				writeLogFile("siftRun gimbalManagerRotate-45 x:%f, y:%f, z:%f\n", imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);
				continue;
			}
			else
			{
				//if(siftHanderStatus == camerScanModeSearch)
				{
					lastGimbalYaw = currentGimbalAngle;
					currentGimbalAngle = getGimbalAnglesYaw();

					if(fabs(currentGimbalAngle - lastGimbalYaw) < 1.0f)
					{
						scanOver = true;
						//recovery the camera status(down and first angle)
						gimbalSetModeAndReset();
						sleepMs(2000);
						differenceGimbalYawAndFlightYaw = getFlightCurrentBearing() - getGimbalAnglesYaw();
						m_needAlignCameraYaw = false;
						//set scan mode false
						m_startCameraScanMode = false;
						writeLogFile("siftRun camera-scan-360, but not successfull.\n");

						//search marker failed to switch auto land for groundstation control copter to other landpoint.
						writeLogFile("siftRun search marker failed to switch auto land for groundstation control copter to other landpoint.\n");
						goToAlternateLandingPoint();

						continue;
					}

					//Rotate 360 degrees clockwise
					cameraScanModeRotateSpeed(GIMBAL_ROTATE_SPEED_ANGLE);
				}
			}
		}

		if(m_ekfInitSuccess && m_startCameraScanMode)
		{
			//reset yaw
			gimbalSetModeAndReset();
			sleepMs(2000);
			differenceGimbalYawAndFlightYaw = getFlightCurrentBearing() - getGimbalAnglesYaw();
			m_needAlignCameraYaw = false;
			//set scan mode false
			m_startCameraScanMode = false;
			scanOver = true;
			writeLogFile("siftRun ekfinit-success-and-close-camera-scan.\n");
			continue;
		}

		cvtColor(im, imGray, CV_RGB2GRAY);

		if(imGray.empty())
		{
			writeLogFile("siftRun  imgray is empty  .....err\n");
			delay = intervalMs;
			continue;
		}

		vector<Point> rect;
		double rationAngle;
		int inOrEx = 0;
		int externalGoodMatchesMinNum;
		int internalGoodMatchesMinNum;
		int returnGoodMatches = 0;

		double flightCurrentHeight = getFlightCurrentHeigh();

		//whether start the camera scan for ekf init
		if(flightCurrentHeight < START_CAMERA_SCAN_MODE && !m_ekfInitSuccess
			&& !scanOver && !m_startCameraScanMode)
		{
			writeLogFile("siftRun start-camera-scan-mode\n");
			m_siftHanderStatus = camerScanModeSearch;
			m_needAlignCameraYaw = true;
			m_startCameraScanMode = true;
			cameraScanModeRotateAbsoluteAngle(-90);
			//Rotate pitch CAMERA_SCAN_GIMBAL_ANGLE, then rotate yaw 90, need 2 sconds.
			sleepMs(2000);
			currentGimbalAngle = getGimbalAnglesYaw();
			cameraScanModeRotateSpeed(GIMBAL_ROTATE_SPEED_ANGLE);
			continue;
		}

		inOrEx = AllQrCode;
		externalGoodMatchesMinNum = 20;
		internalGoodMatchesMinNum = 18;

		if(m_startCameraScanMode)
		{
			externalGoodMatchesMinNum = 15;
			internalGoodMatchesMinNum = 18;
		}

		// record the start time
		std::chrono::steady_clock::time_point startTime = std::chrono::steady_clock::now();

		switch(m_siftHanderStatus)
		{
			case siftMarkerNone:
			case siftLandMarker:
			{
				// land marker sift
				ret = computeLandPosition(imGray, markerExKp, markerExDes, markerInKp, markerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches);
				break;
			}

			case camerScanModeSearch:
			{
				//land marker sift
				ret = computeLandPosition(imGray, markerExKp, markerExDes, markerInKp, markerInDes, rect, rationAngle,
										inOrEx, externalGoodMatchesMinNum, internalGoodMatchesMinNum, returnGoodMatches, false);
				if(ret)
				{
					m_siftHanderStatus = siftLandMarker;
				}
				break;
			}
		}

		//record the finish time
		std::chrono::steady_clock::time_point finishTime = std::chrono::steady_clock::now();

		//compute the  algorithm use time
		double usedTimeMs = std::chrono::duration_cast<std::chrono::duration<double>>(finishTime - startTime).count() * 1e3;
		writeLogFile("siftRun usedTimeMs:%f\n", usedTimeMs);

		if(ret)
		{
			if(m_siftHanderStatus == siftMarkerNone)
			{
				siftCount++;
				if(siftCount > IS_LAND_QR_OK_NUM)
				{
					m_siftHanderStatus = siftLandMarker;
				}
			}

			//compute computeLandBearing
			if(ExternalQrCode == inOrEx && !m_homeBearingLockFlag)
			{
				double flightLandAngle;
				//the gimbal maybe not align with the flight head, so here use gimbal yaw add difference.
				flightLandAngle = imageGimbalAngle.z + rationAngle + differenceGimbalYawAndFlightYaw;
				flightLandAngle = wrap360(flightLandAngle);

				guessBearing.push_back(flightLandAngle);

				if(guessBearing.size() > FLIGHT_BEARING_LOCK_NUM)
				{
					//identify the land bearing.
					computeCorrectBearing(guessBearing, 5, m_computeLandBearing);
					m_homeBearingLockFlag = true;
					writeLogFile("siftRun computeLandBearing:%f is locked\n", m_computeLandBearing);
				}
			}

			//image center
			Point imageCenter(imGray.cols / 2, imGray.rows / 2);
			double positionErr, rotation;
			computeMoveBearing(imageCenter, rect, inOrEx, positionErr, rotation, m_siftHanderStatus);

			writeLogFile("siftRun positionErr:%f, roation:%f, flightCurrentYaw:%f, gimbalAngles.x:%f, gimbalAngles.y:%f, gimbalAngles.z:%f\n",
								positionErr, rotation, imageFlightYaw, imageGimbalAngle.x, imageGimbalAngle.y, imageGimbalAngle.z);

			//according to the gimbalAngles compute the velocity bearing
			double positionBearing;

			//when the flight bearing lost rtk status, the flight yaw is bigger difference than gimbal yaw,
			//so velocity bearing use the gimbal yaw.
			positionBearing = imageGimbalAngle.z + rotation + differenceGimbalYawAndFlightYaw;

			positionBearing = wrap360(positionBearing);

			Vector3f translation;
			getTranslationSum(translation);

			Vector3f positionErrXYZ;

			//compute north position error
			positionErrXYZ.x = positionErr * cos(positionBearing * M_PI / 180) - translation.x;

			//compute east position error
			positionErrXYZ.y = positionErr * sin(positionBearing * M_PI / 180) - translation.y;

			positionErrXYZ.z = 0;

			//assistLand.writeLogFile("siftRun-pos-error1 x:%f, y:%f\n", positionErrXYZ.x, positionErrXYZ.y);

			//because the distance from the center of the external marker to the center of the interal marker is 0.12m 
			if(ExternalQrCode == inOrEx)
			{
				if(m_homeBearingLockFlag)
				{
					double errorAngle = m_computeLandBearing;
					positionErrXYZ.x += CAMARE_TO_FLIGHT_CENTER_DISTANCE * cos(errorAngle * M_PI / 180);
					positionErrXYZ.y += CAMARE_TO_FLIGHT_CENTER_DISTANCE * sin(errorAngle * M_PI / 180);
				}
			}

			if(m_startCameraScanMode)
			{
				double radAngle = fabs(90 + CAMERA_SCAN_GIMBAL_ANGLE) * (M_PI / 180);
				double rotateAddDistance = flightCurrentHeight * tan(radAngle);
				double guessGimbalAngle;
				guessGimbalAngle = imageGimbalAngle.z + differenceGimbalYawAndFlightYaw;
				//tan(fabs(CAMERA_SCAN_GIMBAL_ANGLE)), so the ground distance is equal with flightCurrentHeight
				positionErrXYZ.x += rotateAddDistance * cos(guessGimbalAngle * M_PI / 180);
				positionErrXYZ.y += rotateAddDistance * sin(guessGimbalAngle * M_PI / 180);
			}

			setPositionError(positionErrXYZ);

			//writeLogFile("siftRun-pos-error2 x:%f, y:%f , siftRun positionBearing:%f, translation x:%f, y:%f\n",
				//positionErrXYZ.x, positionErrXYZ.y, positionBearing, translation.x, translation.y);
		}

		//compute the sleep time
		delay = intervalMs - usedTimeMs;
		delay = delay <= 0 ? 2 : delay;
	}

	gimbalSetModeAndReset();
}

bool AssistLand::gimbalSetModeAndReset()
{
	bool ret = m_apmFlightHandler->commandDoGimbalManagerPitchyawNeutral();
	if(ret)
	{
		sleepMs(3000);
	}
	return ret;
}


double AssistLand::getFlightCurrentBearing()
{
	float flightHeading;
	int64_t updateTimeStamp;
	m_apmFlightHandler->getFlgihtHeading(flightHeading, updateTimeStamp);
	return flightHeading;
}


double AssistLand::getGimbalAnglesYaw()
{
	Vector3f gimbalAttitude;
	int64_t updateTimeStamp;
	m_apmFlightHandler->getGimbalAttitude(gimbalAttitude, updateTimeStamp);
	return gimbalAttitude.z;
}


bool AssistLand::siftHandlerMarker(vector<KeyPoint> &markerExKp, Mat &markerExDes, vector<KeyPoint> &markerInKp, Mat &markerInDes, enum siftMarkerType type)
{
	bool ret;
	Mat markerEx, markerIn;

	switch(type)
	{
		case siftLandMarker:
		{
			markerEx = imread("/home/forlinx/hbSource/external_marker.png", IMREAD_GRAYSCALE);
			markerIn = imread("/home/forlinx/hbSource/internal_marker.png", IMREAD_GRAYSCALE);
			break;
		}
		default:
			return false;
	}

	ret = siftHandleImage(markerEx, markerExKp, markerExDes);

	if(!ret)
	{
		writeLogFile("siftHandlerMarker  siftHandleImage external marker err\n");
		m_assistLandRunFunctionFlag = false;
		m_siftRunFunctionFlag = false;
		return false;
	}

	ret = siftHandleImage(markerIn, markerInKp, markerInDes);

	if(!ret)
	{
		writeLogFile("siftHandlerMarker  siftHandleImage internal marker err\n");
		m_assistLandRunFunctionFlag = false;
		m_siftRunFunctionFlag = false;
		return false;
	}

	return true;
}


void AssistLand::resetTranslationSum()
{
	unique_lock<mutex> ulock(m_translationSumLock);
	memset(&m_translationSum, 0, sizeof(Vector3f));
	ulock.unlock();
}


void AssistLand::setTranslationSum(Vector3f translationAdd)
{
	unique_lock<mutex> ulock(m_translationSumLock);
	m_translationSum.x += translationAdd.x;
	m_translationSum.y += translationAdd.y;
	m_translationSum.z += translationAdd.z;
	ulock.unlock();
}

void AssistLand::getTranslationSum(Vector3f &translation)
{
	unique_lock<mutex> ulock(m_translationSumLock);
	memcpy(&translation, &m_translationSum, sizeof(Vector3f));
	ulock.unlock();

	writeLogFile("getTranslationSum x:%f, y:%f, z:%f\n", m_translationSum.x, m_translationSum.y, m_translationSum.z);
}


void AssistLand::setPositionError(Vector3f positionErr)
{
	unique_lock<mutex> ulock(m_positionErrorLock);
	memcpy(&m_positionError, &positionErr, sizeof(Vector3f));
	m_positionErrorUpdateFlag = true;
	ulock.unlock();

	writeLogFile("setPositionError x:%f, y:%f, z:%f\n", m_positionError.x, m_positionError.y, m_positionError.z);
}

void AssistLand::getPositionError(Vector3f &positionErr, bool &updateFlag)
{
	unique_lock<mutex> ulock(m_positionErrorLock);
	updateFlag = m_positionErrorUpdateFlag;
	m_positionErrorUpdateFlag = false;
	memcpy(&positionErr, &m_positionError, sizeof(Vector3f));
	ulock.unlock();
}


bool AssistLand::gimbalRotate()
{
	return m_apmFlightHandler->commandDoGimbalManagerPitchyawYawLock(-90, getFlightCurrentBearing(), 50, 50);
}


bool AssistLand::cameraScanModeRotateAbsoluteAngle(double rotateRelativeAngle)
{
	return m_apmFlightHandler->commandDoGimbalManagerPitchyawYawLock(CAMERA_SCAN_GIMBAL_ANGLE, getFlightCurrentBearing() + 0, 50, 50);
}


bool AssistLand::cameraScanModeRotateSpeed(double rotateRelativeAngle)
{
	float angle = std::numeric_limits<float>::quiet_NaN();
	return m_apmFlightHandler->commandDoGimbalManagerPitchyawYawLock(angle, angle, 0, rotateRelativeAngle);;
}

void AssistLand::computeCorrectBearing(const vector<float> data, double kMeasRadius, double &returnAngle)
{
	int indexMaxNum;
	int indexMax;
	int internCount;

	indexMaxNum = 0;
	//search the center point
	for(int i=0; i<data.size(); i++)
	{
		float tmp = data[i];
		internCount = 0;

		for(int j=0; j<data.size(); j++)
		{
			double error;
			error = data[j] - tmp;

			error = (float)fmod((float)fmod((error + 180), 360) + 360, 360) - 180;

			if(fabsf(error) < kMeasRadius)
			{
				internCount++;
			}
		}

		if(internCount > indexMaxNum)
		{
			indexMaxNum = internCount;
			indexMax = i;
		}
	}

	float center;
	float distanceSum;

	distanceSum = 0;
	center = data[indexMax];
	internCount = 0;

	for(int i=0; i<data.size(); i++)
	{
		double error;
		error = data[i] - center;
		error = (float)fmod((float)fmod((error + 180), 360) + 360, 360) - 180;

		if(fabsf(error) < kMeasRadius)
		{
			distanceSum += error;
			internCount++;
		}
	}

	returnAngle = center + distanceSum / (float)internCount;
}


void AssistLand::computeMoveBearing(Point imageCenter, vector<Point> rect, int inOrEx, double &positionErr, double &rotationAngle, enum siftMarkerType type)
{
	//col error, x is col
	int errX = rect[rect.size()-1].x - imageCenter.x;

	//row error, y is row
	int errY = rect[rect.size()-1].y - imageCenter.y;

	//atan2 scale from - pi to pi
	double angle = atan2(fabsf(errY), fabsf(errX)) * (180.0f / M_PI);

	if(errX > 0 && errY < 0)
	{
		//one area
		angle = 90 - angle;
	}
	else if(errX > 0 && errY > 0)
	{
		//two area
		angle += 90 ;
	}
	else if(errX < 0 && errY > 0)
	{
		//three area
		angle = 270 - angle;
	}
	else if(errX < 0 && errY < 0)
	{
		//four area
		angle += 270;
	}

	positionErr = sqrt(pow(errX,2) + pow(errY, 2));
	rotationAngle = angle;

	float widthEdge, heightEdge;
	widthEdge = sqrt(pow(rect[1].x - rect[0].x, 2) + pow(rect[1].y - rect[0].y, 2));
	heightEdge = sqrt(pow(rect[2].x - rect[1].x, 2) + pow(rect[2].y - rect[1].y, 2));

	float ratio;
	float realLongth = 0.0f;

	switch(inOrEx)
	{
		case ExternalQrCode:
		{
			realLongth = EXTERNAL_LONG_EDGE;
			break;
		}
		case InternalQrCode:
		{
			realLongth = INTERNAL_LONG_EDGE;
			break;
		}
	}

	ratio =  widthEdge > heightEdge ? realLongth / widthEdge : realLongth / heightEdge;

	positionErr *= ratio;
}

void AssistLand::getFlightVelocity(Vector3f &velocity, int64_t &timestamp)
{
	m_apmFlightHandler->getGlobalVelocity(velocity, timestamp);
}

float AssistLand::controlVelocityZ()
{
	double flightCurrentHeight = getFlightCurrentHeigh();
	float velocityZ = 0.0f;

	if(flightCurrentHeight > 30.0f)
	{
		//down speed set -1.5m/s
		velocityZ = -1.5f;
	}
	else if(flightCurrentHeight > 10.0f)
	{
		//down speed set -1m/s
		velocityZ = -1.0f;
	}
	else if(flightCurrentHeight > 5.0f)
	{
		//down speed set -1m/s
        velocityZ = -0.5f;
	}
	else
	{
		//down speed set -0.3m/s
		velocityZ = -0.3f;
	}

	if(m_startCameraScanMode)
	{
		if(getBatteryInfoPercentage() > LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO)
		{
			//wait assist land init about 5s
			velocityZ = 0.0f;
		}
	}

	return velocityZ;
}

void AssistLand::controlVelocityXY(PID &txPid, PID &tyPid, Vector3f error, Vector3f &velocity)
{
	//compute the pid
	velocity.x = txPid.getPid(error.x);
	velocity.y = tyPid.getPid(error.y);
}

double AssistLand::controlYaw(PID &yawPid, double &angleError)
{
	double yawRate;
	if(m_homeBearingLockFlag)
	{
		double computeLandBearing, flightCurrentYaw, flightAngleErr;
		computeLandBearing = m_computeLandBearing;
		flightCurrentYaw = getFlightCurrentBearing();

		flightAngleErr = computeLandBearing - flightCurrentYaw;
		flightAngleErr = (float)fmod((float)fmod((flightAngleErr + 180), 360) + 360, 360) - 180;

		//compute the pid
		yawRate = yawPid.getPid(flightAngleErr);
		angleError = flightAngleErr;
	}
	else
	{
		yawRate = 0;
		yawPid.resetI();
		angleError = 0;
	}

	return yawRate;
}


uint8_t AssistLand::getBatteryInfoPercentage()
{
	return LOITER_COPTER_FOR_ASSIST_LAND_INIT_BATTERYIFO + 1;
}


bool AssistLand::velocityControlToCopter(Vector3f velocity, double yawRate)
{
	return m_apmFlightHandler->velocityControlToCopter(velocity, yawRate);
}


MAV_LANDED_STATE AssistLand::getFlightStatus()
{
	int64_t updateTimeStamp;
	MAV_LANDED_STATE landStatus;
	m_apmFlightHandler->getLandStatus(landStatus, updateTimeStamp);

	return landStatus;
}


void AssistLand::guidedModeHandler()
{
	if(m_goToAlternateLandPoint == true && m_setLandPosStatus == notInit)
	{
		if(m_apmFlightHandler->setLandPoistionToFlight())
		{
			m_setLandPosStatus = setSuccess;
			m_targetPositionLastDistatceUpdateTimeStamp = getTimeMillis();
			writeLogFile("guidedModeHandler setLandPoistionToFlight success\n");
		}
		else
		{
			m_setLandPosStatus = setFailed;
			writeLogFile("guidedModeHandler setLandPoistionToFlight failed\n");
			if(!m_apmFlightHandler->setFlightMode(RTL))
			{
				writeLogFile("guidedModeHandler setFlightMode rtl failed\n");
				if(!m_apmFlightHandler->setFlightMode(LAND))
				{
					writeLogFile("guidedModeHandler setFlightMode land failed\n");
				}
			}
		}
	}
	else if(m_goToAlternateLandPoint == true && m_setLandPosStatus == setSuccess)
	{
		//get current position
		Vector3f distance;
		int64_t updateTimeStamp;
		m_apmFlightHandler->getTargetPositionDistance(distance, updateTimeStamp);
		if(updateTimeStamp > m_targetPositionLastDistatceUpdateTimeStamp)
		{
			m_targetPositionLastDistatceUpdateTimeStamp = updateTimeStamp;
			float distanceError = sqrt(pow(distance.x, 2) + pow(distance.y, 2) + pow(distance.z, 2));

			if(distance.x < 0.5f)
			{
				if(!m_apmFlightHandler->setFlightMode(LAND))
				{
					// sorry I don't know why
					writeLogFile("guidedModeHandler setFlightMode land22 failed\n");
				}
				else
				{
					m_setLandPosStatus = notInit;
				}
			}
		}
	}
}


AssistLand assistLand;
